#include "ros/ros.h"
#include "ssc32.hpp"
#include "corobot_srvs/SetSpeed.h"
#include "corobot_ssc32/MoveArm.h"
#include "corobot_ssc32/AvancerReculer.h"
#include "corobot_ssc32/GauchDroit.h"
#include "corobot_ssc32/HautBas.h"
#include "corobot_msgs/ssc32_info.h"
#include "corobot_msgs/MotorCommand.h"
#include "corobot_msgs/ServoPosition.h"
#include "bra_util.hpp"
#include <math.h>


using namespace Servo;

SSC32 m_ssc32;
Armind::BraUtil m_braUtil;

std::string SSC32_PORT;
ros::Timer timer;


bool SetSpeed(corobot_srvs::SetSpeed::Request  &req, corobot_srvs::SetSpeed::Response &res )
{
	m_ssc32.SendMessage(1,req.left_speed,req.speed);
	m_ssc32.SendMessage(0,req.right_speed,req.speed);	
	return true;
}


void timerCallback(const ros::TimerEvent&)
{
	m_ssc32.SendMessage(1,0,0);
	m_ssc32.SendMessage(0,0,0);
}

void SetSpeedTopic(const corobot_msgs::MotorCommand::ConstPtr &msg)
{
	ros::NodeHandle n;
	m_ssc32.SendMessage(1,(msg->leftSpeed*-5)+1500,(100-msg->acceleration)*10);
	m_ssc32.SendMessage(0,(msg->rightSpeed*-5)+1500,(100-msg->acceleration)*10);
	timer = n.createTimer(ros::Duration(msg->secondsDuration), timerCallback, true);
}

bool MoveArm(corobot_ssc32::MoveArm::Request  &req, corobot_ssc32::MoveArm::Response &res )
{
	ROS_INFO("MoveArm\n");
  if(req.shoulderOrientation != 0)
  {
	
	//m_ssc32.SendMessage(4,amount,2000);
	m_ssc32.SendMessage(1,1500,2000);

	return true;
  }

  if(req.elbowOrientation != 0)
  {
	int amount =  (int) ((req.elbowOrientation) / M_PI * 1500) + 1500 - 1500 / 2;
	//m_ssc32.SendMessage(5,amount,2000);
	m_ssc32.SendMessage(2,amount,2000);

	return true;
  }
  if(req.baseOrientation != 0)
  {
	int amount =  (int) (req.baseOrientation / M_PI * 1500) + 1500 - 1500 / 2;
	//m_ssc32.SendMessage(4,amount,2000);
	m_ssc32.SendMessage(0,amount,2000);

	return true;
  }
  return false;

}

bool MoveWrist(corobot_ssc32::MoveArm::Request  &req, corobot_ssc32::MoveArm::Response &res )
{	
	int amount =  (int) (req.wristOrientation / M_PI * 2000) + 1000 - 2000 / 2;
	//m_ssc32.SendMessage(6,amount,2000);
	m_ssc32.SendMessage(3,amount,2000);
	return true;
}

bool ResetArm(corobot_ssc32::MoveArm::Request  &req, corobot_ssc32::MoveArm::Response &res )
{
	
	m_ssc32.SendMessage(0,1500,2000);
	m_ssc32.SendMessage(1,1500,2000);
	m_ssc32.SendMessage(2,1500,2000);
	m_ssc32.SendMessage(3,1500,2000);
	m_ssc32.SendMessage(4,1500,2000);

	/*///////////////
	int amount =  (int) (req.baseOrientation / M_PI * 1500) + 1500 - 1500 / 2;
	m_ssc32.SendMessage(0,amount,2000);
	//////////////

	amount =  (int) (req.shoulderOrientation / M_PI * 1500) + 1500 - 1500 / 2;
	m_ssc32.SendMessage(1,amount,2000);

	amount =  (int) (req.elbowOrientation / M_PI * 1500) + 1500 - 1500 / 2;
	m_ssc32.SendMessage(2,amount,2000);*/
	return true;
}

bool MoveGripper(corobot_ssc32::MoveArm::Request  &req, corobot_ssc32::MoveArm::Response &res )
{
		int amount;		
		if (req.gripper == req.GRIPPER_OPEN)
			amount = 1000;
		else if (req.gripper == req.GRIPPER_CLOSED)
			amount = 2000;
		m_ssc32.SendMessage(4,amount,2000);
	
		return true;
}

bool Gauch ()
{
	int pos = m_braUtil.calculatePoisitonForGauche();
	m_ssc32.SendMessage(0,pos,1000);
	return true;
}
bool Droit ()
{
	int pos = m_braUtil.calculatePoisitonForDroite();
	m_ssc32.SendMessage(0,pos,1000);
	return true;
}
bool Haut ()
{
	int pos = m_braUtil.calculatePoisitonForHaut();
	m_ssc32.SendMessage(2,pos,1000);
	return true;
}
bool Bas ()
{
	int pos = m_braUtil.calculatePoisitonForBas();
	m_ssc32.SendMessage(2,pos,1000);
	return true;
}
bool Avancer ()
{
	int pos1 = m_braUtil.calculatePoisitonOfShoulderForAvancer();
	int pos2 = m_braUtil.calculatePoisitonOfArmForAvancer();
	m_ssc32.SendMessage(1,pos1,1000);
	m_ssc32.SendMessage(2,pos2,1000);
	return true;
}
bool Reculer ()
{
	int pos1 = m_braUtil.calculatePoisitonOfShoulderForReculer();
	int pos2 = m_braUtil.calculatePoisitonOfArmForReculer();
	m_ssc32.SendMessage(1,pos1,1000);
	m_ssc32.SendMessage(2,pos2,1000);
	return true;
}


bool AvancerReculer(corobot_ssc32::AvancerReculer::Request &req, corobot_ssc32::AvancerReculer::Response &res){
	if(req.is_avancer == req.AVANCER) {
		Avancer();
	}else if (req.is_avancer == req.RECULER) {
		Reculer();
	}else {
		return false;
	}
	return true;
}

bool GauchDroit(corobot_ssc32::GauchDroit::Request &req, corobot_ssc32::GauchDroit::Response &res){
	if(req.is_gauch == req.GAUCH) {
		Gauch();
	}else if (req.is_gauch == req.DROIT) {
		Droit();
	}else {
		return false;
	}
	return true;
}

bool HautBas(corobot_ssc32::HautBas::Request &req, corobot_ssc32::HautBas::Response &res){
	if(req.is_haut == req.HAUT) {
		Haut();
	}else if (req.is_haut == req.BAS) {
		Bas();
	}else {
		return false;
	}
	return true;
}

/**
 * @brief Callback for the topic /phidgetServo_setPosition
	  The position of the concerned servo motor is set each time a message is received on this topic.
 * @param corobot_msgs::ServoPosition Message
 */ 
void setPositionCallback(const corobot_msgs::ServoPosition &msg)
{
	int amount =  (int) (msg.position / 180 * 1500) + 1500 - 1500 / 2;  //ideally, msg.position is between 0 and 150 degrees
	bool ok = m_ssc32.SendMessage(msg.index,amount,2000);
	if (!ok)
		ROS_ERROR("Could not set the servo motor number %d to the position %f",msg.index, msg.position);
}



int main(int argc, char **argv)
{
	ros::init(argc, argv, "ssc32_node");
	ros::NodeHandle n("~");

	//Declare services
	ros::ServiceServer speed = n.advertiseService("ssc32_set_speed", SetSpeed);
	ros::ServiceServer arm = n.advertiseService("/move_arm", MoveArm);
	ros::ServiceServer wrist = n.advertiseService("/move_wrist", MoveWrist);
	ros::ServiceServer gripper = n.advertiseService("/move_gripper", MoveGripper);
	ros::ServiceServer arm_reset = n.advertiseService("/reset_arm", ResetArm);
	ros::Publisher ssc32_info_pub = n.advertise<corobot_msgs::ssc32_info>("ssc32_info", 1000);
	
	ros::ServiceServer hb = n.advertiseService("/haut_bas", HautBas);
	ros::ServiceServer ar = n.advertiseService("/avancer_reculer", AvancerReculer);
	ros::ServiceServer gd = n.advertiseService("/gauch_droit", GauchDroit);

	//Subscribe to topics
	ros::Subscriber velocity=n.subscribe<corobot_msgs::MotorCommand>("/ssc32_velocity",1000, SetSpeedTopic);
	ros::Subscriber position_sub = n.subscribe("phidgetServo_setPosition",100, &setPositionCallback);

	std::string ssc32_port_;

   	n.param<std::string>("ssc32_port", ssc32_port_, "/dev/ttyUSB0");

    	if(!n.getParam("ssc32_port", ssc32_port_))
    		ROS_INFO("can not get parameter ssc32_port!");

	SSC32_PORT = ssc32_port_;
	
	m_ssc32.initSerial();

	if(m_ssc32.startSerial(SSC32_PORT))
	{
	  ROS_INFO("Success!!! Connect to serial port");
	  corobot_msgs::ssc32_info info;
	  info.connected = 1;
	  ssc32_info_pub.publish(info);
	}

//	m_ssc32.nrOfPorts = 4;	

//	m_ssc32.SendMessage(1,0,0);
//	m_ssc32.SendMessage(0,0,0);	

	//main ros function
        ros::spin();
	
	ROS_INFO("Out of ros spin!!!");
		//we don't want to use the ssc32 after that.
        m_ssc32.stopSerial();
        
	return 0;
}
